package edu.colorado.cs.correll.cudroplet.shaperecognition;

import java.util.HashSet;

import com.vividsolutions.jts.geom.Coordinate;

import processing.core.PApplet;
import processing.core.PVector;

public abstract class Robot {
	ShapeRecognition shapeRecognition;
	int id;
	float x;
	float y;
	float theta;

	RobotState state = RobotState.SEARCH;

	Leader leader = null;
	Leader alphaLeader = null;

	public Robot(ShapeRecognition shapeRecognition, int id, float x, float y,
			float theta) {
		this.shapeRecognition = shapeRecognition;
		this.id = id;
		this.x = x;
		this.y = y;
		this.theta = theta;
	}

	abstract public void update();

	public Coordinate getCoordinate() {
		return new Coordinate(x, y);
	}

	public PVector getPVector() {
		return new PVector(x, y);
	}

	int getPix() {
		return shapeRecognition.pixels[PApplet.floor(y)
				* shapeRecognition.width + PApplet.floor(x)];
	}

	void detectBoundary(int pix) {
		// detect only blue boundaries
		if (pix == shapeRecognition.color(0, 0, 255)) {
			setState(RobotState.FOUND);
		}
	}

	void setState(RobotState s) {
		if (s == RobotState.FOUND) {
			state = RobotState.FOUND;
			shapeRecognition.simulation.swarm.neighbors[id] = new HashSet<Robot>();
			shapeRecognition.simulation.swarm.tree.insert(getCoordinate(), this);
		}
	}
}
